
//  Copyright:NPU Soccer Base Humanoid group
//  Author:Zhang Duo
//  Date:2019.2.18
//  Entry to the whole program 


#include <transform.h>

#include <Robot.h>
//Used for UART on raspberry
#include <wiringPi.h>
#include <wiringSerial.h>

int main(int argc, char const *argv[])
{
    // Transform* trf = new Transform();
    // trf->controller();
    // trf->printOut();
    // std::cout <<"Cost:"<<trf->getCost()<<std::endl;
    // delete trf;

    // Transform* test = new Transform();
    // test->mainControl(0);
    // std::cout <<"minCost: "<<test->getMinCost()<<std::endl;
    // test->printFinalOut();
    // test->printCharResult();
    // delete test;

    int fd;
    unsigned char *message;

    Transform* test1 = new Transform();
    test1->AdjustForImage();
    test1->mainControl(3);

    if (wiringPiSetup() < 0)
        return 1;
    if ((fd = serialOpen((char *)"/dev/ttyS0", 115200)) < 0)
        return 1;

    std::cout <<"minCost: "<<std::dec<<test1->getMinCost()<<std::endl;
    message = test1->getMessage();
    serialPrintf(fd, (char *)message);
    serialPutchar(fd, '\0');
    printf("Done!");
    serialClose(fd);
    //test1->printFinalOut();
    //test1->printCharResult();
    delete test1;
    
    return 0;
}
